#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <stdbool.h>
#include "ili9320.h"
#include "global.h"
#include "interrupt.h"
#include "ff.h"
#include "screen_io.h"
#include "screen_font.h"
#include "gcode.h"
#include "parse.h"
#include "file.h"
#include "keyboard.h"
#include "display.h"
#include "gcode_exec.h"
#include "gcode_control.h"
#include "rs232_interface.h"
#include "global_conf.h"



// #define NO_ACCELERATION_CORRECTION

bool isPause = false;
int curGCodeMode;
uint32_t startWorkTime;


uint8_t isExtruderOn;

extern const char axisName[5];

const double axisK[4] =
{
    SM_X_STEPS_PER_MM,
    SM_Y_STEPS_PER_MM,
    SM_Z_STEPS_PER_MM,
    SM_E_STEPS_PER_MM
};

#define TABLE_CENTER_X  (MAX_TABLE_SIZE_X / 4)
#define TABLE_CENTER_Y  (MAX_TABLE_SIZE_Y / 4)



double k_scr;
short prev_scrX, prev_scrY;

short crdXtoScr(double x)
{
    return (short)(x * k_scr + 0.5) + 8;
}

short crdYtoScr(double y)
{
    short v = (short)(y * k_scr + 0.5);
    return v > (LCD_HEIGHT - 1) ? 0 : LCD_HEIGHT - 1 - v;
}


void GcodeProc_Init(void)
{
    double kx = ((double)(LCD_WIDTH - 10)) / MAX_TABLE_SIZE_X;
    double ky = ((double)(LCD_HEIGHT - 2)) / MAX_TABLE_SIZE_Y;
    k_scr = kx > ky ? ky : kx;
    LCD_Clear(Black);

    memset(&lbuf, 0, sizeof(linesBuffer));

#ifndef NO_ACCELERATION_CORRECTION
    lbuf.mvectPtrCur = 0;
    lbuf.mvector[0].isNullCmd = true;
    lbuf.mvectCnt = 1;
#endif

    prev_scrX = crdXtoScr(TABLE_CENTER_X);
    prev_scrY = crdYtoScr(TABLE_CENTER_Y);

    lbuf.stepsFromStartX = 0;
    lbuf.stepsFromStartY = 0;
    lbuf.stepsFromStartZ = 0;
    lbuf.stepsFromStartE = 0;
    lbuf.stepsX = 0;
    lbuf.stepsY = 0;
    lbuf.stepsZ = 0;


    isExtruderOn = false;

    minX = 0;
    maxX = 0;
    minY = 0;
    maxY = 0;
    minZ = 0;
    maxZ = 0;

    gc_init();
    commonTimeIdeal = commonTimeReal = 0;
    isGcodeStop = false;
    startWorkTime = Seconds();
}

//---------------------------------------------------------------------
char cncFileBuf[16000];   //文件缓冲

#if (USE_KEYBOARD == 2)
const TPKey_t TPPause = TPKEY(0, 220, 319, 239, 0, NULL);
const TPKey_t kbdGFileC = TPKEY(0, 220, 76, 239, KEY_C, "CANCEL");
const TPKey_t kbdGFileA = TPKEY(84, 220, 156, 239, KEY_A, "PAUSE");
#if (USE_ENCODER == 1)
const TPKey_t kbdGFile0 = TPKEY(164, 220, 236, 239, KEY_0, "ON ENC");
const TPKey_t kbdGFile1 = TPKEY(244, 220, 319, 239, KEY_1, "OFF ENC");
#endif

const TPKey_p kbdGFile[] =
{
    &TPPause,
    &kbdGFileC,
    &kbdGFileA,
#if (USE_ENCODER == 1)
    & kbdGFile0,
    &kbdGFile1,
#endif
    NULL
};
#endif


void cnc_gfile(char* fileName, int mode)
{
    int lineNum;
    uint8_t hasMoreLines;

    GcodeProc_Init();

    FRESULT	 res = f_open(&fid, fileName, FA_READ);
    if (res != FR_OK)
    {
        win_showErrorWin();
        scr_printf("Error open file:'%s'\nStatus:%d", fileName, (int)res);
        return;
    }
    curGCodeMode = mode;
    gc.totalfsize = fileList[currentFile].f_size;
    if ((curGCodeMode & GFILE_MODE_MASK_SHOW) != 0)
    {
        scr_Rectangle(crdXtoScr(0), crdYtoScr(MAX_TABLE_SIZE_Y), crdXtoScr(MAX_TABLE_SIZE_X), crdYtoScr(0), Red, false);
        scr_Line(prev_scrX, 30, prev_scrX, 240 - 30, Green);
        scr_Line(50, prev_scrY, 320 - 50, prev_scrY, Green);
    }
    if ((curGCodeMode & GFILE_MODE_MASK_EXEC) != 0)
    {
        char filTittle[40] = "Executing:";
        strcat(filTittle, fileName);
        win_showMenu(0, 50, 40, 13, filTittle);
        scr_fontColor(Blue, Black);
        scr_gotoxy(1, 13);
        scr_puts("C-Cancel  A-Pause 0/1-encoder");
        //       SetTouchKeys(kbdGFile);
    }

    lineNum = 1;
    hasMoreLines = true;
    //将读取到的文件填充到cncFileBuf里
    do
    {
        char* p = cncFileBuf, * str;
        // 1.读取缓冲区
        while (true)
        {
            *p = 0;
            str = p + 1;
            if ((cncFileBuf + sizeof(cncFileBuf) - str) < (MAX_STR_SIZE + 1))   //cncFileBuf剩余的字节数小于最大的一行G代码大小
            {
                break;
            }
            if (f_gets(str, MAX_STR_SIZE, &fid) == NULL)//will break when reached '\n'，读取一行数据
            {
                //besides all gcode line is less than 150 characters
                hasMoreLines = false;
                break;
            }
            str_trim(str);   //将'\r' '\n'转换为0
            *p = (uint8_t)strlen(str) + 1;
            p += *p + 1;
        }
        DBG_DEBUG("filebuf:%s", cncFileBuf);
        for (p = cncFileBuf; !isGcodeStop && *p != 0; lineNum++, p += *p + 1)
        {
            uint8_t st;

            str = p + 1;
            if ((curGCodeMode & GFILE_MODE_MASK_EXEC) != 0)
            {
                GCODE_CMD* gp;

                lbuf.gcodePtrCur++;
                if (lbuf.gcodePtrCur > (MAX_SHOW_GCODE_LINES - 1))
                {
                    lbuf.gcodePtrCur = 0;
                }
                gp = &lbuf.gcode[lbuf.gcodePtrCur];
                strcpy(gp->cmd, str);

                gc.curfsize += strlen(gp->cmd);
                gp->lineNum = lineNum;
            }

            DBG_DEBUG("[gcode:%d] %s", lineNum, str);
            /*执行一行G代码*/
            st = gc_execute_line(str);
            DBG_DEBUG("st:%d", st);
            ShowGcodeExeState();
            if (st != GCSTATUS_OK)
            {
                ShowGcodeErrorStare(st, lineNum, str);
                f_close(&fid);
                return;
            }
        }
    } while (!isGcodeStop && hasMoreLines);

    f_close(&fid);

    if ((curGCodeMode & GFILE_MODE_MASK_EXEC) == 0)
    {
        short scrX = crdXtoScr(TABLE_CENTER_X);
        short scrY = crdYtoScr(TABLE_CENTER_Y);
        int t1 = commonTimeIdeal / 1000;
        int t2 = commonTimeReal / 1000;

        scr_Line(scrX - 8, scrY, scrX + 8, scrY, Red);
        scr_Line(scrX, scrY - 8, scrX, scrY + 8, Red);
        scr_fontColor(Green, Black);
        scr_gotoxy(1, 0);
        scr_printf("Time %02d:%02d:%02d(%02d:%02d:%02d) N.cmd:%d",
            t1 / 3600, (t1 / 60) % 60, t1 % 60,
            t2 / 3600, (t2 / 60) % 60, t2 % 60,
            lineNum
        );
        scr_printf("\n X%f/%f Y%f/%f Z%f/%f", minX, maxX, minY, maxY, minZ, maxZ);
    }
    else
    {
#ifndef NO_ACCELERATION_CORRECTION
        mc_line(0, 0, 0, 0, 0, 0);
        mc_line(0, 0, 0, 0, 0, 0);
#endif
    }
}


//=================================================================================================================================

uint8_t cnc_waitSMotorReady(void)
{
#if (USE_STEP_DEBUG == 1)
    static uint8_t isStepDump = false;
#endif
    int i;

    do
    {
        if (stepm_getRemainLines() > 1)
        {
            scr_fontColor(Black, Cyan);
            for (i = 0; i < STEPS_MOTORS; i++)
            {
                int32_t globalSteps = stepm_getCurGlobalStepsNum(i);
                double n = (double)globalSteps / axisK[i];
                scr_gotoxy(1 + i * 10, 6);
                scr_printf("%c:%f ", axisName[i], n);
            }
#if (USE_STEP_DEBUG == 1)
            if (isStepDump)
            {
                step_dump();
            }
#endif
        }

        switch (kbd_GetKey())
        {
        case KEY_C_DOWN:
            stepm_EmergeStop();
            return false;
        case KEY_U_DOWN:
            isPause = true;
            DBG_INFO("User CMD:pause!");
            break;
        case KEY_D_DOWN:
            scr_gotoxy(1, 6);
            scr_clrEndl();
            break;
        case KEY_L_DOWN:
#if (USE_STEP_DEBUG == 1)
            isStepDump = true;
#endif
            break;
        case KEY_R_DOWN:
#if (USE_STEP_DEBUG == 1)
            isStepDump = false;
#endif
            break;
        }
        DBG_TRACE("LineBufStatus:%d", stepm_LinesBufferIsFull());
    } while (stepm_LinesBufferIsFull());

    if (limits_chk())
    {
        stepm_EmergeStop();
        scr_fontColor(Red, Black);
        scr_gotoxy(1, 10);
        scr_puts("LIMITS ERROR!");
        scr_clrEndl();
        return false;
    }
    return true;
}

#if (USE_KEYBOARD == 2)
const TPKey_t TPPauseB = TPKEY(0, 220, 156, 239, KEY_B, "CONTINUE");
const TPKey_t TPPauseC = TPKEY(164, 220, 319, 239, KEY_C, "CANCEL");
const TPKey_p kbdPause[] =
{
    &TPPause,
    &TPPauseB,
    &TPPauseC,
    NULL
};
#endif

//准备将一行代码添加到代码缓冲区中
//暂停处理
//代码显示
//打印调试信息
//将dx,fx,dirx装换成 stepsx,freqx,dirx 添加到步进电机执行缓冲队列里
uint8_t sendLine(uint32_t fxyze[], uint32_t abs_dxyze[], uint8_t dir_xyze[])
{
    uint32_t f = 0;
    uint32_t i, n = 0;

    for (i = 0; i < 3; i++)
    {
        if (fxyze[i] > f)
        {
            f = fxyze[i];
            n = i;
        }
    }//获取xyz轴中最大的f,n为最大的轴
    // if(abs_dxyze[n] > 20) {
    //  for(i = 0; i < 4; i++) fxyze[i] = (uint32_t)((uint64_t)f * (uint64_t)abs_dxyze[i]/abs_dxyze[n]);
    if (abs_dxyze[n] > 10)
    {
        for (i = 0; i < 4; i++)
        {
            if (i != n)
            {
                fxyze[i] = (uint32_t)(((uint64_t)f * (uint64_t)abs_dxyze[i]) / abs_dxyze[n]) + 1; //+1 for debug
            }
        }
    }
    //>***********显示G代码，但是不执行**********
    if ((curGCodeMode & GFILE_MODE_MASK_EXEC) == 0)
    {
        int i;
        for (i = 0; i < 4; i++)
        {
            if (fxyze[i] != 0)
            {
                commonTimeReal += abs_dxyze[i] * 1000L * K_FRQ / fxyze[i];
                break;
            }
        }
        if ((curGCodeMode & GFILE_MODE_MASK_SHOW) != 0)
        {
            if (dir_xyze[0])
            {
                lbuf.stepsX += abs_dxyze[0];
            }
            else
            {
                lbuf.stepsX -= abs_dxyze[0];
            }

            if (dir_xyze[1])
            {
                lbuf.stepsY += abs_dxyze[1];
            }
            else
            {
                lbuf.stepsY -= abs_dxyze[1];
            }

            if (dir_xyze[2])
            {
                lbuf.stepsZ += abs_dxyze[2];
            }
            else
            {
                lbuf.stepsZ -= abs_dxyze[2];
            }

            double x, y;
            short scrX, scrY;

            x = (double)lbuf.stepsX / SM_X_STEPS_PER_MM;
            y = (double)lbuf.stepsY / SM_Y_STEPS_PER_MM;

            scrX = crdXtoScr(TABLE_CENTER_X + x);
            scrY = crdYtoScr(TABLE_CENTER_Y + y);

            if (scrX != prev_scrX || prev_scrY != scrY)
            {
                scr_Line(prev_scrX, prev_scrY, scrX, scrY, calcColor((uint8_t)(lbuf.stepsZ * 5 / SM_X_STEPS_PER_MM) & 0x1F));
            }
            prev_scrX = scrX;
            prev_scrY = scrY;
        }
        return true;
    }
    //**********************<	
    DBG_DEBUG("sendLine dx:%c%d(%d)\tdy:%c%d(%d)\tdz:%c%d(%d) tz:%d ty:%d tz:%d",
        dir_xyze[0] ? '+' : '-', abs_dxyze[0], fxyze[0],
        dir_xyze[1] ? '+' : '-', abs_dxyze[1], fxyze[1],
        dir_xyze[2] ? '+' : '-', abs_dxyze[2], fxyze[2],
        fxyze[0] == 0 ? 0 : abs_dxyze[0] * 1000L * K_FRQ / fxyze[0],
        fxyze[1] == 0 ? 0 : abs_dxyze[1] * 1000L * K_FRQ / fxyze[1],
        fxyze[2] == 0 ? 0 : abs_dxyze[2] * 1000L * K_FRQ / fxyze[2]);
    {
        static uint32_t prevF[3] = { 0, 0, 0 };
        static uint8_t prevDir[3] = { 0, 0, 0 };
        int i;

        for (i = 0; i < 3; i++)
        {
            double p;
            if ((p = labs(prevF[i] - fxyze[i]) / (double)_smParam.smoothStartF_from0[i]) > 1.5)
            {
                p *= 100;
                DBG_DEBUG(" !!!!!! <%d:%d/%d\t%f> %s", i, labs(prevF[i] - fxyze[i]), _smParam.smoothStartF_from0[i], p, p > 250 ? "$$$" : "");
            }
            if (prevDir[i] != dir_xyze[i] && (p = fxyze[i] / (double)_smParam.smoothStartF_from0[i]) > 1.5)
            {
                p *= 100;
                DBG_DEBUG(" !!!!!! ## <%d:%d/%d\t%f> %s", i, labs(prevF[i] - fxyze[i]), _smParam.smoothStartF_from0[i], p, p > 250 ? "$$$" : "");
            }
            if (fxyze[i] > _smParam.maxFeedRate[i])
            {
                DBG_DEBUG(" !!!!!! <%d:%d/%d too hight feedrate!> ", i, fxyze[i], _smParam.maxFeedRate[i]);
            }
            if (fxyze[i] < (50 * SM_X_STEPS_PER_MM / 60) && abs_dxyze[i] > 120)
            {
                DBG_DEBUG(" !!!!!! <%d:%d too slow feedrate!> ", i, fxyze[i]);
            }
        }
        for (i = 0; i < 3; i++)
        {
            prevF[i] = fxyze[i];
            prevDir[i] = dir_xyze[i];
        }
    }

    if (!cnc_waitSMotorReady())//reach limit
    {
        return false;
    }
    //***********暂停处理*********************
    if (isPause)
    {
#if (USE_KEYBOARD != 0)
#if (USE_KEYBOARD == 2)
        const TPKey_p* tp_save;
        tp_save = SetTouchKeys(kbdPause);
#endif
#if (USE_KEYBOARD == 1)
        scr_fontColor(Black, White);
        scr_gotoxy(1, 11);
        scr_puts(" PAUSE..'B'-continue 'C'-cancel");
        scr_clrEndl();
#endif
        while (stepm_inProc())
        {
        }
        stepm_EmergeStop();

        while (isPause)
        {
            switch (kbd_GetKey())
            {
            case KEY_C_DOWN:
                isPause = false;
#if (USE_KEYBOARD == 2)
                SetTouchKeys(tp_save);
#endif
                return false;
            case KEY_R_DOWN:
                isPause = false;
                break;
            default:
                break;
            }
        }

#if (USE_KEYBOARD == 1)
        scr_fontColor(White, Black);
        scr_gotoxy(1, 13);
        scr_clrEndl();
#endif
#if (USE_KEYBOARD == 2)
        SetTouchKeys(tp_save);
#endif
#endif
    }
    stepm_addMove(abs_dxyze, fxyze, dir_xyze);
    return true;
}

void cnc_extruder_stop(void)
{
    isExtruderOn = false;
    cnc_waitSMotorReady();
    uint8_t dir[4] = { 0, 0, 0, 1 };
    uint32_t steps[4] = { 0, 0, 0, SM_E_STEPS_PER_MM / 2 };
    uint32_t frq[4] =
    {
        SM_MANUAL_MODE_STEPS_PER_SEC * K_FRQ,
        SM_MANUAL_MODE_STEPS_PER_SEC * K_FRQ,
        SM_MANUAL_MODE_STEPS_PER_SEC * K_FRQ,
        SM_MANUAL_MODE_STEPS_PER_SEC * K_FRQ
    };
    stepm_addMove(steps, frq, dir);
}

void cnc_extruder_on(void)
{
    isExtruderOn = true;
}

//set extrunder temperature
void cnc_extruder_t(int temperature, int isWait)
{
    extrudT_setTemperature((int16_t)temperature);    //set extruder temperature
    if (isWait)
    {
        if ((curGCodeMode & GFILE_MODE_MASK_EXEC) != 0)
        {
            scr_fontColor(Red, Black);
            while (!extrudT_isReady())    //wait while extruder is OK
            {
                scr_gotoxy(3, 2);
                scr_fontColor(Yellow, Blue);
                scr_printf("extruder t:%03d/%03d", extrudT_getTemperatureReal(), extrudT_getTemperatureWait());    //(current temperature/waited temperature)
                if (kbd_GetKey() == KEY_C_DOWN) //return if KEY_C is pressed
                {
                    return;
                }
            }
            scr_gotoxy(3, 2);
            scr_fontColor(White, Black);
            scr_puts("                        ");
        }
    }
}

